//
// Created by javier on 3/11/15.
//

#include "simulator_control.h"
#include "GPS_L1_CA.h"
#include "datetime.h"
#include "geodetic.h"
#include <boost/lexical_cast.hpp>
#include <gflags/gflags.h>
#include <volk/volk.h>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <random>
#include <thread>
#include <utility>


DEFINE_bool(duplicate_satellites, false, "Duplicate each visible satellite in the scenario with its consecutive PRN number (Each satellite will transmit 2 PRNs, for receiver specific observables unit test purposes)");


simulator_control::simulator_control()
{
    d_samp_freq = 2.6e6;
    rinex_obs_writer.d_dump = true;
    rinex_obs_writer.open_dump_file("obs_out.bin");
    d_signal_power_lin = 1;
    d_signal_power_lin = 1;
    d_CN0_dBHz = std::numeric_limits<double>::infinity();
    d_enable_noise = false;
}


simulator_control::~simulator_control() = default;


int simulator_control::readNmeaGGA(std::string &filename)
{
    FILE *fp;
    int numd = 0;
    char str[100]; /*! \brief Maximum length of a line in a text file (RINEX, motion) */
    char *token;
    double llh[3], pos[3];
    char tmp[8];
    std::vector<double> tmp_xyz(3);

    if (nullptr == (fp = fopen(filename.c_str(), "rt")))
        {
            return (-1);
        }

    while (true)
        {
            if (fgets(str, 100, fp) == nullptr)
                {
                    break;
                }

            token = strtok(str, ",");

            if (strncmp(token + 3, "GGA", 3) == 0)
                {
                    token = strtok(nullptr, ",");  // Date and time

                    token = strtok(nullptr, ",");  // Latitude
                    strncpy(tmp, token, 2);
                    tmp[2] = 0;

                    llh[0] = atof(tmp) + atof(token + 2) / 60.0;

                    token = strtok(nullptr, ",");  // North or south
                    if (token[0] == 'S')
                        {
                            llh[0] *= -1.0;
                        }

                    llh[0] /= GPS_RAD2DEG;  // in radian

                    token = strtok(nullptr, ",");  // Longitude
                    strncpy(tmp, token, 3);
                    tmp[3] = 0;

                    llh[1] = atof(tmp) + atof(token + 3) / 60.0;

                    token = strtok(nullptr, ",");  // East or west
                    if (token[0] == 'W')
                        {
                            llh[1] *= -1.0;
                        }

                    llh[1] /= GPS_RAD2DEG;  // in radian

                    token = strtok(nullptr, ",");  // GPS fix
                    token = strtok(nullptr, ",");  // Number of satellites
                    token = strtok(nullptr, ",");  // HDOP

                    token = strtok(nullptr, ",");  // Altitude above meas sea level

                    llh[2] = atof(token);

                    token = strtok(nullptr, ",");  // in meter

                    token = strtok(nullptr, ",");  // Geoid height above WGS84 ellipsoid

                    llh[2] += atof(token);

                    // Convert geodetic position into ECEF coordinates
                    llh2xyz(llh, pos);
                    tmp_xyz.at(0) = pos[0];
                    tmp_xyz.at(1) = pos[1];
                    tmp_xyz.at(2) = pos[2];
                    d_obs_xyz.push_back(tmp_xyz);

                    // Update the number of track points
                    numd++;

                    //if (numd>=3000) /*! \brief Maximum number of user motion waypoints */
                    //    break;
                }
        }

    fclose(fp);

    return (numd);
}

int simulator_control::readUserMotion(std::string &filename)
{
    std::vector<double> tmp_xyz(3);
    FILE *fp;
    int numd;
    /*! \brief Maximum length of a line in a text file (RINEX, motion) */
    char str[100];
    double t, x, y, z;

    if (nullptr == (fp = fopen(filename.c_str(), "rt")))
        {
            return (-1);
        }

    numd = 0;
    while (true)
        {
            if (fgets(str, 100, fp) == nullptr)
                { /*! \brief Maximum length of a line in a text file (RINEX, motion) */
                    break;
                }

            if (EOF == sscanf(str, "%lf,%lf,%lf,%lf", &t, &x, &y, &z))
                {  // Read CSV line
                    break;
                }

            tmp_xyz.at(0) = x;
            tmp_xyz.at(1) = y;
            tmp_xyz.at(2) = z;
            d_obs_xyz.push_back(tmp_xyz);
            numd++;
        }

    fclose(fp);

    return (numd);
}


bool simulator_control::open_out_file(std::string out_file)
{
    if (d_out_filestream.is_open() == false)
        {
            try
                {
                    d_out_filename = std::move(out_file);
                    d_out_filestream.exceptions(std::ifstream::failbit | std::ifstream::badbit);
                    d_out_filestream.open(d_out_filename.c_str(), std::ios::out | std::ios::binary);
                    //LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
                    return true;
                }
            catch (const std::ifstream::failure &e)
                {
                    //LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
                    return false;
                }
        }
    else
        {
            return false;
        }
}

/*! \brief Replace all 'E' exponential designators to 'D'
 *  \param str String in which all occurrences of 'E' are replaced with *  'D'
 *  \param len Length of input string in bytes
 *  \returns Number of characters replaced
 */
int simulator_control::replaceExpDesignator(char *str, int len)
{
    int i, n = 0;

    for (i = 0; i < len; i++)
        {
            if (str[i] == 'D')
                {
                    n++;
                    str[i] = 'E';
                }
        }

    return (n);
}

bool simulator_control::open_rinex_obs_file(std::string obs_file)
{
    return rinex_obs_writer.open_obs_file(std::move(obs_file));
}


bool simulator_control::read_eph(const std::string &eph_file)
{
    /*! \brief Read Ephemersi data from the RINEX Navigation file */
    /*  \param[out] eph Array of Output SV ephemeris data
   *  \param[in] fname File name of the RINEX file
   *  \returns Number of SV found in the file, -1 on error
   */
    FILE *fp;
    int nsat;
    int sv;
    char str[100];
    char tmp[20];

    datetime t;
    gpstime g;
    gpstime g0;
    double dt;

    if (nullptr == (fp = fopen(eph_file.c_str(), "rt")))
        {
            return false;
        }

    //skip header

    while (true)
        {
            if (nullptr == fgets(str, 100, fp))
                {
                    break;
                }

            if (0 == strncmp(str + 60, "END OF HEADER", 13))
                {
                    break;
                }
        }

    nsat = 0;
    g0.week = -1;

    Gps_Ephemeris gps_eph;

    while (true)
        {
            // PRN / EPOCH / SV CLK
            if (nullptr == fgets(str, 100, fp))
                {
                    break;
                }

            strncpy(tmp, str + 3, 2);
            tmp[2] = 0;
            t.y = atoi(tmp) + 2000;

            strncpy(tmp, str + 6, 2);
            tmp[2] = 0;
            t.m = atoi(tmp);

            strncpy(tmp, str + 9, 2);
            tmp[2] = 0;
            t.d = atoi(tmp);

            strncpy(tmp, str + 12, 2);
            tmp[2] = 0;
            t.hh = atoi(tmp);

            strncpy(tmp, str + 15, 2);
            tmp[2] = 0;
            t.mm = atoi(tmp);

            strncpy(tmp, str + 18, 4);
            tmp[2] = 0;
            t.sec = atof(tmp);

            g = t.date2gps();

            t.gps2date(&g);

            if (g0.week == -1)
                {
                    g0 = g;
                }

            dt = g.sec - g0.sec;

            if ((g.week == g0.week) && (dt > -3600.0) && (dt <= 3600.0))
                {
                    strncpy(tmp, str, 2);
                    tmp[2] = 0;
                    sv = atoi(tmp);

                    if (d_gps_eph_map.read(sv, gps_eph) == false)  //no eph record found for this satellite
                        {
                            gps_eph.i_satellite_PRN = sv;
                            gps_eph.d_Toc_gpstime = g;

                            strncpy(tmp, str + 22, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);  // tmp[15]='E';
                            gps_eph.d_A_f0 = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_A_f1 = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_A_f2 = atof(tmp);

                            // BROADCAST ORBIT - 1
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 3, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_IODE_SF2 = (int)atof(tmp);

                            strncpy(tmp, str + 22, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Crs = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Delta_n = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_M_0 = atof(tmp);

                            // BROADCAST ORBIT - 2
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 3, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Cuc = atof(tmp);

                            strncpy(tmp, str + 22, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_e_eccentricity = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Cus = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_sqrt_A = atof(tmp);

                            // BROADCAST ORBIT - 3
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 3, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Toe_gpstime.sec = atof(tmp);

                            strncpy(tmp, str + 22, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Cic = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_OMEGA0 = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Cis = atof(tmp);

                            // BROADCAST ORBIT - 4
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 3, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_i_0 = atof(tmp);

                            strncpy(tmp, str + 22, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Crc = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_OMEGA = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_OMEGA_DOT = atof(tmp);

                            // BROADCAST ORBIT - 5
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 3, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_IDOT = atof(tmp);

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_Toe_gpstime.week = (int)atof(tmp);

                            // BROADCAST ORBIT - 6
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }

                            strncpy(tmp, str + 41, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_TGD = atof(tmp);

                            strncpy(tmp, str + 60, 19);
                            tmp[19] = 0;
                            replaceExpDesignator(tmp, 19);
                            gps_eph.d_IODC = (int)atof(tmp);

                            // BROADCAST ORBIT - 7
                            if (nullptr == fgets(str, 100, fp))
                                {
                                    break;
                                }
                            nsat++;
                            d_gps_eph_map.write(sv, gps_eph);  //insert the new ephemeris
                        }
                }
            else
                {
                    break;
                }
        }

    fclose(fp);

    return true;
}

bool simulator_control::setsamplingfreq(double freq)
{
    if (freq > (2 * GPS_L1_CA_CODE_RATE_HZ))
        {
            d_samp_freq = freq;
            return true;
        }
    else
        {
            return false;
        }
}

bool simulator_control::run()
{
    ////////////////////////////////////////////////////////////
    // Receiver position
    ////////////////////////////////////////////////////////////

    if (d_obs_xyz.size() == 0)
        {
            std::cout << "No observer positions found, check OBSERVER positions file!" << std::endl;
            return false;
        }
    else
        {
            // Initial location in Geodetic coordinate system
            double xyz[3];
            double llh[3];
            xyz[0] = d_obs_xyz.at(0)[0];
            xyz[1] = d_obs_xyz.at(0)[1];
            xyz[2] = d_obs_xyz.at(0)[2];
            xyz2llh(xyz, llh);  //in RADIANS!
            std::cout << "Observer starting coordinates ECEF= " << d_obs_xyz.at(0)[0] << " , " << d_obs_xyz.at(0)[1] << " , " << d_obs_xyz.at(0)[2] << std::endl;
            std::cout << "Observer starting coordinates Geodetic Lat= " << llh[0] * GPS_RAD2DEG << ", Long= " << llh[1] * GPS_RAD2DEG << ", H=" << llh[2] << std::endl;
        }

    std::map<int, Gps_Ephemeris> gps_eph_map_cpy;
    if (FLAGS_duplicate_satellites)
        {
            std::cout << "WARNING: Duplicated satellites mode enabled!\n";
            std::cout << "WARNING: Do not use the generated signal file for computing conventional PVT solution.\n";
            std::vector<int> prn_list;
            std::map<int, Gps_Ephemeris> tmp_eph_map_cpy;
            tmp_eph_map_cpy = d_gps_eph_map.get_map_copy();
            for (std::map<int, Gps_Ephemeris>::iterator it = tmp_eph_map_cpy.begin(); it != tmp_eph_map_cpy.end(); ++it)
                {
                    //if the current prn and the adjacent prn are not in the prn lists, then, add the satellite and its dupplicated
                    if (std::find(prn_list.begin(), prn_list.end(), it->second.i_satellite_PRN) == prn_list.end() and
                        std::find(prn_list.begin(), prn_list.end(), it->second.i_satellite_PRN + 1) == prn_list.end() and (it->second.i_satellite_PRN + 1) < 33)
                        {
                            std::cout << "Duplicated satellite PRN ID pairs " << it->second.i_satellite_PRN << "," << it->second.i_satellite_PRN + 1 << std::endl;
                            //original satellite
                            prn_list.push_back(it->second.i_satellite_PRN);
                            gps_eph_map_cpy.insert(std::make_pair(it->second.i_satellite_PRN, it->second));
                            //duplicated one
                            it->second.i_satellite_PRN++;
                            prn_list.push_back(it->second.i_satellite_PRN);
                            gps_eph_map_cpy.insert(std::make_pair(it->second.i_satellite_PRN, it->second));
                        }
                }
        }
    else
        {
            gps_eph_map_cpy = d_gps_eph_map.get_map_copy();
        }


    //get initial GPS time from first ephemeris in the ephemeris map
    if (d_gps_eph_map.size() == 0)
        {
            std::cout << "No ephemeris found, check RINEX navigation file!" << std::endl;
            return false;
        }
    gpstime g0;

    g0 = gps_eph_map_cpy.begin()->second.d_Toe_gpstime;
    g0.sec = (double)(((unsigned long)g0.sec) / 30UL) * 30.0;  // align with the full frame length = 30 sec

    std::cout << "Start Time = " << g0.week << ", " << g0.sec << std::endl;

    ////////////////////////////////////////////////////////////
    // CREATE GPS CHANNELS
    ////////////////////////////////////////////////////////////

    for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map_cpy.begin(); it != gps_eph_map_cpy.end(); ++it)
        {
            d_gps_l1_ca_channels_vec.push_back((std::shared_ptr<gps_l1_ca_sim_channel_cc>)new gps_l1_ca_sim_channel_cc(it->second, d_samp_freq));
            d_gps_l1_ca_channels_vec.back()->set_cn0(d_enable_noise, d_signal_amplitude_lin);
            trk_obs_writer_vec.push_back((std::shared_ptr<tracking_obs_writer>)new tracking_obs_writer());
            //Open true observations log files
            std::string obs_log_filename = "gps_l1_ca_obs_prn";
            obs_log_filename.append(boost::lexical_cast<std::string>(it->second.i_satellite_PRN));
            obs_log_filename.append(".dat");
            trk_obs_writer_vec.back()->open_obs_file(obs_log_filename);
        }

    int iq_buff_size;
    iq_buff_size = (int)floor(d_samp_freq / 10.0);  // samples per 0.1sec
    double delt;
    delt = 1.0 / d_samp_freq;

    std::complex<float> **gps_iq_ch_buffers;
    gps_iq_ch_buffers = new std::complex<float> *[d_gps_l1_ca_channels_vec.size()];


    int nsatgps = 0;
    for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin(); it != d_gps_l1_ca_channels_vec.end(); ++it)
        {
            //std::cout<<"Generating GPS PRN CODE and TELEMETRY for SV "<<(*it)->d_eph.i_satellite_PRN<<std::endl;
            (*it)->codegen();  // generate local PRN codes inside channels
            gps_iq_ch_buffers[nsatgps] = static_cast<std::complex<float> *>(volk_malloc(iq_buff_size * sizeof(std::complex<float>), volk_get_alignment()));

            (*it)->set_output_buffer(gps_iq_ch_buffers[nsatgps], iq_buff_size);
            nsatgps++;
        }

    ////////////////////////////////////////////////////////////
    // Baseband signal buffer and output file
    ////////////////////////////////////////////////////////////

    // Allocate I/Q buffer
    std::complex<float> *iq_buff;
    std::complex<float> *iq_buff_noise;
    lv_8sc_t *iq_buff_quantized;
    iq_buff_quantized = static_cast<lv_8sc_t *>(volk_malloc(iq_buff_size * sizeof(lv_8sc_t), volk_get_alignment()));
    iq_buff = static_cast<std::complex<float> *>(volk_malloc(iq_buff_size * sizeof(std::complex<float>), volk_get_alignment()));
    iq_buff_noise = static_cast<std::complex<float> *>(volk_malloc(iq_buff_size * sizeof(std::complex<float>), volk_get_alignment()));

    if (iq_buff_quantized == nullptr or iq_buff == nullptr)
        {
            std::cout << "Faild to allocate IQ buffers" << std::endl;
            return false;
        }

    if (d_out_filestream.is_open() == false)
        {
            std::cout << "Could not write on baseband output file" << std::endl;
            return false;
        }

    ////////////////////////////////////////////////////////////
    // Initialize channels
    ////////////////////////////////////////////////////////////
    double el_mask_deg = 0.0;
    // Initial reception time
    gpstime grx;
    grx = g0;

    for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin(); it != d_gps_l1_ca_channels_vec.end(); ++it)
        {
            (*it)->generate_telemetry(g0);
            (*it)->update_AOA(d_obs_xyz.at(0), g0);
            (*it)->init_carrier_phase_and_range(g0, d_obs_xyz.at(0));
            if ((*it)->d_el_deg > el_mask_deg)
                {
                    std::cout << (*it)->d_eph.i_satellite_PRN << "  " << (*it)->d_az_deg << "  " << (*it)->d_el_deg << std::endl;
                }
        }

    std::cout << "Write RINEX OBS header..." << std::endl;
    datetime init_datetime;
    init_datetime.gps2date(&g0);
    rinex_obs_writer.write_obs_header("(28-OCT-15 00:00)   ", d_obs_xyz.at(0), 1.0, init_datetime);


    ////////////////////////////////////////////////////////////
    // Generate baseband signals
    ////////////////////////////////////////////////////////////
    std::cout << "Generating baseband signals [multithread]..." << std::endl;

    bool printed_initial_doppler = false;
    //TODO: The simulation time step is HARDCODED to 0.1s in a number of functions.
    //      It is used also internally in the gps_l1_ca_sim_channel to compute the Doppler frequency.
    //      Fix this to enable flexible simulation time step
    grx.sec += 0.1;  // the simulation time step
    unsigned int obs_counter = 0;
    int sv_prns[d_gps_l1_ca_channels_vec.size()];
    int sv_channel_number[d_gps_l1_ca_channels_vec.size()];
    double sv_dist_m[d_gps_l1_ca_channels_vec.size()];
    double sv_true_dist_m[d_gps_l1_ca_channels_vec.size()];
    double sv_acc_phase_hz[d_gps_l1_ca_channels_vec.size()];
    double sv_acc_phase_l2_hz[d_gps_l1_ca_channels_vec.size()];
    double doppler_l1_hz[d_gps_l1_ca_channels_vec.size()];

    std::thread *satellite_threads;

    satellite_threads = new std::thread[d_gps_l1_ca_channels_vec.size()];


    std::default_random_engine d_random_generator;
    std::normal_distribution<float> d_Gaussian;  //defaults to mean 0 and stddev 1

    for (std::vector<std::vector<double>>::iterator obs_pos = d_obs_xyz.begin(); obs_pos != d_obs_xyz.end(); ++obs_pos)
        {
            if (obs_counter != 0)  //skip first obs position (it is used as the initial observer point)
                {
                    int channel_count = 0;
                    for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin();
                         it != d_gps_l1_ca_channels_vec.end(); ++it)
                        {
                            // Update observables for this satellite (multithread)
                            satellite_threads[channel_count] = std::thread(&gps_l1_ca_sim_channel_cc::update_observables,
                                *it, *obs_pos, grx, g0);
                            channel_count++;
                        }

                    channel_count = 0;
                    for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin();
                         it != d_gps_l1_ca_channels_vec.end(); ++it)
                        {
                            satellite_threads[channel_count].join();
                            if ((*it)->d_el_deg > el_mask_deg)
                                {  //process only the satellites with positive elevations
                                    //generate baseband signal for the satellite and for this epoch
                                    satellite_threads[channel_count] = std::thread(&gps_l1_ca_sim_channel_cc::generate_signal, *it);
                                }
                            channel_count++;
                        }

                    channel_count = 0;
                    int visible_sats_count = 0;
                    for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin();
                         it != d_gps_l1_ca_channels_vec.end(); ++it)
                        {
                            if ((*it)->d_el_deg > el_mask_deg)
                                {  //process only the satellites with positive elevations
                                    satellite_threads[channel_count].join();
                                    if (printed_initial_doppler == false)
                                        {
                                            std::cout << "Sat " << (*it)->d_eph.i_satellite_PRN
                                                      << " initial Doppler " << (*it)->get_doppler()
                                                      << " [Hz] initial Code Phase " << (*it)->get_code_phase() << " [Chips]" << std::endl;
                                        }

                                    //write binary dump for Tracking observables (only visible satellites)
                                    trk_obs_writer_vec.at(channel_count)->write_binary_obs((*it)->get_current_timestamp(), (*it)->get_acc_carrier_phase(), (*it)->get_doppler(), (*it)->get_code_phase(),
                                        (*it)->d_current_TOW + 0.1);  //todo: remove hardcoded simulation step
                                    sv_channel_number[visible_sats_count] = channel_count;
                                    sv_prns[visible_sats_count] = (*it)->d_eph.i_satellite_PRN;
                                    sv_dist_m[visible_sats_count] = (*it)->get_pseudorange();
                                    sv_true_dist_m[visible_sats_count] = (*it)->get_true_range();
                                    sv_acc_phase_hz[visible_sats_count] = (*it)->get_acc_carrier_phase();
                                    sv_acc_phase_l2_hz[visible_sats_count] = (*it)->get_acc_carrier_phase_l2();
                                    doppler_l1_hz[visible_sats_count] = (*it)->get_doppler();
                                    visible_sats_count++;
                                }
                            channel_count++;
                        }
                    printed_initial_doppler = true;

                    //write BINARY DUMP Epoch
                    rinex_obs_writer.write_binary_epoch(visible_sats_count,
                        sv_prns,
                        &grx,
                        sv_dist_m,
                        sv_true_dist_m,
                        sv_acc_phase_hz,
                        sv_acc_phase_l2_hz,
                        doppler_l1_hz);
                    //write RINEX OBS EPOCH
                    rinex_obs_writer.write_obs_epoch(visible_sats_count,
                        sv_prns,
                        &grx,
                        sv_dist_m,
                        sv_acc_phase_hz,
                        sv_acc_phase_l2_hz,
                        doppler_l1_hz);

                    // produce output signal
                    memset(iq_buff, 0, iq_buff_size * sizeof(std::complex<float>));  //clear output accumulator buffer

                    //Sum all the satellite signals
                    // todo: Use Volk operator here
                    for (int n = 0; n < iq_buff_size; n++)
                        {
                            for (std::vector<std::shared_ptr<gps_l1_ca_sim_channel_cc>>::iterator it = d_gps_l1_ca_channels_vec.begin();
                                 it != d_gps_l1_ca_channels_vec.end(); ++it)
                                {
                                    //elevation mask filter
                                    if ((*it)->d_el_deg > el_mask_deg)
                                        {
                                            iq_buff[n] += (*it)->d_output_buffer[n];
                                        }
                                }
                            //Add noise if required
                            if (d_enable_noise)
                                {
                                    iq_buff_noise[n] = std::complex<float>(d_Gaussian(d_random_generator) / sqrt(2), d_Gaussian(d_random_generator) / sqrt(2));
                                }
                        }

                    //Add noise if required
                    if (d_enable_noise)
                        {
                            for (int n = 0; n < iq_buff_size; n++)
                                {
                                    iq_buff[n] += iq_buff_noise[n];
                                }
                        }

                    if (d_enable_noise and visible_sats_count > 0)
                        {
                            //DEBUG: SNR and CN0 report
                            double Ps_lin = 0;
                            double Pn_lin = 0;
                            double SNR;
                            double SNR_db;
                            double CN0;

                            for (int n = 0; n < iq_buff_size; n++)
                                {
                                    Ps_lin += std::real(d_gps_l1_ca_channels_vec.at(sv_channel_number[0])->d_output_buffer[n] * std::conj(d_gps_l1_ca_channels_vec.at(sv_channel_number[0])->d_output_buffer[n]));
                                    Pn_lin += std::real(iq_buff_noise[n] * std::conj(iq_buff_noise[n]));
                                }

                            Ps_lin = Ps_lin / static_cast<float>(iq_buff_size);
                            Pn_lin = Pn_lin / static_cast<float>(iq_buff_size);
                            SNR = Ps_lin / Pn_lin;     // Carrier to Noise ratio
                            SNR_db = 10 * log10(SNR);  // Carrier to Noise ratio (dB)
                            CN0 = 10 * log10(SNR * this->d_samp_freq);

                            std::cout << " True Ps_lin: " << Ps_lin << " Pn_lin: " << Pn_lin << " CN0: " << CN0 << " [dB-Hz]" << std::endl;
                        }
                    else
                        {
                            if (visible_sats_count == 0)
                                {
                                    std::cout << "WARNING: No visible satellites for the simulated receiver position\n";
                                }
                        }
                    //Quantize
                    int nbits = 4;
                    const float SIGNAL_GAIN = pow(2, 4);  //for quantization
                    for (int n = 0; n < iq_buff_size; n++)
                        {
                            iq_buff_quantized[n] = lv_8sc_t(static_cast<int8_t>(SIGNAL_GAIN * iq_buff[n].real()),
                                static_cast<int8_t>(SIGNAL_GAIN * iq_buff[n].imag()));
                        }
                    // write sample stream to file or device
                    d_out_filestream.write((char *)iq_buff_quantized, iq_buff_size * sizeof(lv_8sc_t));
                    // increase simulation time
                    grx.sec += 0.1;

                    printf("\rTime = %4.1f", grx.sec - g0.sec);
                    fflush(stdout);
                }
            obs_counter++;
        }


    // FREE MEMORY
    for (int n = 0; n < d_gps_l1_ca_channels_vec.size(); n++)
        {
            delete gps_iq_ch_buffers[n];
        }

    delete gps_iq_ch_buffers;
    delete[] satellite_threads;
    return true;
}

void simulator_control::setstaticposition(double lat_deg, double lon_deg, double h, unsigned int num_points)
{
    double llh[3];
    double xyz[3];
    llh[0] = lat_deg / GPS_RAD2DEG;
    llh[1] = lon_deg / GPS_RAD2DEG;
    llh[2] = h;
    llh2xyz(llh, xyz);  // Convert llh to xyz

    std::vector<double> tmp_xyz(3);
    for (unsigned int n = 0; n < num_points; n++)
        {
            tmp_xyz.at(0) = xyz[0];
            tmp_xyz.at(1) = xyz[1];
            tmp_xyz.at(2) = xyz[2];
            d_obs_xyz.push_back(tmp_xyz);
        }
}

void simulator_control::setCN0(double CN0_dBHz)
{
    if (CN0_dBHz != std::numeric_limits<double>::infinity())
        {
            d_enable_noise = true;
            d_signal_power_lin = pow(10.0, CN0_dBHz / 10.0) / (d_samp_freq / 2.0);
            d_signal_power_lin = d_signal_power_lin / 2;  //Signal power is equally splitted in the I and Q components
            d_signal_amplitude_lin = sqrt(d_signal_power_lin);
            std::cout << "Gaussian noise generator enabled. CN0 set to " << CN0_dBHz << " [dB-Hz]" << std::endl;
            std::cout << "GPS signal amplitude set to " << d_signal_amplitude_lin << std::endl;
        }
    else
        {
            std::cout << "Noise generator disabled. CN0 is not set." << std::endl;
            d_enable_noise = false;
        }
}
